﻿/*
 * @Author       : xiaoyuan-cloud 1805980417@qq.com
 * @Date         : 2024-10-21 09:55:19
 * @LastEditors  : xiaoyuan-cloud 1805980417@qq.com
 * @LastEditTime : 2024-10-24 14:42:56
 * @FilePath     : \charger_controller\packages\SAE_J1939\src\J1939.c
 * @Description  : SAE_J1939协议栈
 * SPDX-License-Identifier: MIT
 * Copyright (c) 2024 by xiaoyuan-cloud, All Rights Reserved.
 */

#include "j1939.h"
#include "j1939_config.h"
#include "string.h"

 /*缓存队列*/
typedef struct
{
	j1939_uint8_t Node_Addr;			   // 节点地址
	j1939_uint8_t Queue_Count;			   // 队列数量
	j1939_uint8_t Head;					   // 队列头部数据(出队列)
	j1939_uint8_t Tail;					   // 队列尾部数据(入队列)
	J1939_MESSAGE Queue[J1939_QUEUE_SIZE]; // 队列数据缓冲区
} CAN_BUF_QUEUE;

/*TP组帧全局变量*/
typedef struct
{
	J1939_TP_Flags TP_Flag;			   // 组帧状态标志
	J1939_TRANSPORT_TX_INFO TP_TX_MSG; // 组帧发送消息
	J1939_TRANSPORT_RX_INFO TP_RX_MSG; // 组帧接收消息
} TP_TEMP_Value;

/*单帧报文(用于组帧报文应答)*/
J1939_MESSAGE OneMessage;

/*CAN1节点缓存队列*/
CAN_BUF_QUEUE can1_tx_queue;
CAN_BUF_QUEUE can1_rx_queue;
// CAN1 TP组帧数据
TP_TEMP_Value can1_tp_value;

/*CAN2节点缓存队列*/
CAN_BUF_QUEUE can2_tx_queue;
CAN_BUF_QUEUE can2_rx_queue;
// CAN2 TP组帧数据
TP_TEMP_Value can2_tp_value;

/*CAN3节点缓存队列*/
CAN_BUF_QUEUE can3_tx_queue;
CAN_BUF_QUEUE can3_rx_queue;

/*CAN4节点缓存队列*/
CAN_BUF_QUEUE can4_tx_queue;
CAN_BUF_QUEUE can4_rx_queue;

/**
 * @brief 软件过滤节点地址
 * @param {CAN_NODE} port	CAN端口
 * @param {J1939_MESSAGE*} pthis	消息对象
 * @return {*}
 * @retval 0: 验证成功
 * @retval -1: 验证失败
 */
int Message_Filter(CAN_NODE port, J1939_MESSAGE* pthis)
{
	switch (port)
	{
	case Select_CAN_NODE_1:
	{
		if ((pthis->j1939_pid.PDUFormat < 240) && (pthis->j1939_pid.PDUSpecific == J1939_STARTING_ADDRESS_1))
		{
			return RC_SUCCESS;
		}
		break;
	}
	case Select_CAN_NODE_2:
	{
		if ((pthis->j1939_pid.PDUFormat < 240) && (pthis->j1939_pid.PDUSpecific == J1939_STARTING_ADDRESS_2))
		{
			return RC_SUCCESS;
		}
		break;
	}
	case Select_CAN_NODE_3:
	{
		if ((pthis->j1939_pid.PDUFormat < 240) && (pthis->j1939_pid.PDUSpecific == J1939_STARTING_ADDRESS_3))
		{
			return RC_SUCCESS;
		}
		break;
	}
	case Select_CAN_NODE_4:
	{
		if ((pthis->j1939_pid.PDUFormat < 240) && (pthis->j1939_pid.PDUSpecific == J1939_STARTING_ADDRESS_4))
		{
			return RC_SUCCESS;
		}
		break;
	}
	default:
		break;
	}
	//printf("5\n");
	return RC_ERROR;
}

/**
 * @brief 发送单帧报文
 * @param {CAN_NODE} port CAN端口
 * @param {J1939_MESSAGE*} pthis 消息对象
 * @return {*}
 * @retval 0: 发送成功
 * @retval -1: 发送失败
 */
int SendOneMessage(CAN_NODE port, J1939_MESSAGE* pthis)
{
	pthis->j1939_pid.Res = 0; // 参考J1939的数据链路层（SAE J1939-21）
	pthis->j1939_pid.RTR = 0;
	if (pthis->j1939_pid.DataLength > 8)
		pthis->j1939_pid.DataLength = 8;

	return J1939_CAN_Transmit(port, pthis);
}

/**
 * @brief 读取报文消息(单帧组帧处理 中断调用)
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 */
void J1939_ReceiveMessage(CAN_NODE port)
{
    //printf("1\n");
	j1939_uint32_t _pgn = 0; // 校验和转存pgn变量
	j1939_uint32_t len = 0;	 // 数据长度

	/*读取数据*/
	if (J1939_CAN_Receive(port, &OneMessage) == RC_SUCCESS)
	{
	    //printf("2\n");
		/*校验地址*/
		if (Message_Filter(port, &OneMessage) != RC_SUCCESS)
		{
		    //printf("3\n");
			return;
		}

		/*链接管理*/
		if (OneMessage.j1939_pid.PDUFormat == J1939_PF_TP_CM)
		{
			/*计算pgn*/
			_pgn = (j1939_uint32_t)((OneMessage.j1939_pid.Data[7] << 16) & 0xFF0000) +
				(j1939_uint32_t)((OneMessage.j1939_pid.Data[6] << 8) & 0xFF00) + (j1939_uint32_t)((OneMessage.j1939_pid.Data[5]) & 0xFF);
			/*计算数据长度*/
			len = (j1939_uint32_t)((OneMessage.j1939_pid.Data[2] << 8) & 0xFF00) + (j1939_uint32_t)(OneMessage.j1939_pid.Data[1] & 0xFF);

			/*CAN端口*/
			switch (port)
			{
				/*CAN1通道*/
			case Select_CAN_NODE_1:
				/*判断TP连接状态、接收状态空闲*/
				if ((can1_tp_value.TP_Flag.state == J1939_TP_NULL) && (can1_tp_value.TP_RX_MSG.state == J1939_TP_RX_WAIT))
				{
					/*判断控制字为TP_CM_RTS*/
					if (OneMessage.j1939_pid.Data[0] == J1939_RTS_CONTROL_BYTE)
					{
						can1_tp_value.TP_Flag.state = J1939_TP_RX; // 设置为接收状态

						can1_tp_value.TP_RX_MSG.tp_rx_msg.SA = OneMessage.j1939_pid.SourceAddress; // 转存源地址
						can1_tp_value.TP_RX_MSG.tp_rx_msg.PGN = _pgn;							   // 转存PGN参数组编号
						/*是否有足够的内存接收数据*/
						if (len > J1939_TP_MAX_MESSAGE_LENGTH)
						{
							can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_ERROR; // 断开连接接收状态设置为连接错误
							return;
						}
						can1_tp_value.TP_RX_MSG.tp_rx_msg.byte_count = len;					  // 数据长度
						can1_tp_value.TP_RX_MSG.packets_total = OneMessage.j1939_pid.Data[3]; // 数据包数
						can1_tp_value.TP_RX_MSG.time = J1939_TP_T2;							  // 连接管理超时时间
						can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_READ_DATA;				  // 设置接收状态为读取数据状态(DT)
					}
				}
				/*发送组帧*/
				else if (can1_tp_value.TP_Flag.state == J1939_TP_TX)
				{
					/*校验接收方PGN*/
					if (_pgn == can1_tp_value.TP_RX_MSG.tp_rx_msg.PGN)
					{
						switch (OneMessage.j1939_pid.Data[0])
						{
						case J1939_RTS_CONTROL_BYTE:
							/* 程序运行到这里，说明已经与网络中设备1建立虚拟链接（作为发送端），但是收到设备2的链接请求，并且同一个PGN消息请求*/
							/* 根据J1939-21数据链路层的规定，我们要保持原有的链接，不做任何事，设备2会应为超时自动放弃链接*/
							break;
						case J1939_CTS_CONTROL_BYTE:
							if ((can1_tp_value.TP_TX_MSG.state == J1939_TP_TX_CM_WAIT) || (can1_tp_value.TP_TX_MSG.state == J1939_TP_WAIT_ACK))
							{
								/*接收方能接收的包数*/
								if (OneMessage.j1939_pid.Data[1] == 0x00u)
								{
									/*刷新等待计数器*/
									can1_tp_value.TP_TX_MSG.tim = J1939_TP_T4;
								}
								else
								{
									/*请求数据包范围*/
									if ((OneMessage.j1939_pid.Data[2] + OneMessage.j1939_pid.Data[1]) > (can1_tp_value.TP_TX_MSG.packet_total + 1))
									{
										/*超出包范围，发送状态错误*/
										can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_ERROR;
									}
									else
									{
										/*响应请求参数*/
										can1_tp_value.TP_TX_MSG.packets_request_num = OneMessage.j1939_pid.Data[1];
										can1_tp_value.TP_TX_MSG.packet_offset_p = (j1939_uint8_t)(OneMessage.j1939_pid.Data[2] - 1);
										can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_DT;
									}
								}
							}
							break;
						case J1939_ACK_CONTROL_BYTE:
							if (can1_tp_value.TP_TX_MSG.state == J1939_TP_WAIT_ACK)
							{
								/*发送完成*/
								can1_tp_value.TP_TX_MSG.state = J1939_TX_DONE;
							}
							break;
						case J1939_ABORT_CONTROL_BYTE:
							// 收到放弃连接，等待协议主动放弃链接
							break;
						default:
							break;
						}
					}
				}

				/*单帧数据处理*/
				goto PutInReceiveQueue;
				break;
				/*CAN2通道*/
			case Select_CAN_NODE_2:
				if ((can2_tp_value.TP_Flag.state == J1939_TP_NULL) && (can2_tp_value.TP_RX_MSG.state == J1939_TP_RX_WAIT))
				{
					/*判断控制字为TP_CM_RTS*/
					if (OneMessage.j1939_pid.Data[0] == J1939_RTS_CONTROL_BYTE)
					{
						can2_tp_value.TP_Flag.state = J1939_TP_RX; // 设置为接收状态

						can2_tp_value.TP_RX_MSG.tp_rx_msg.SA = OneMessage.j1939_pid.SourceAddress; // 转存源地址
						can2_tp_value.TP_RX_MSG.tp_rx_msg.PGN = _pgn;							   // 转存PGN参数组编号
						/*是否有足够的内存接收数据*/
						if (len > J1939_TP_MAX_MESSAGE_LENGTH)
						{
							can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_ERROR; // 断开连接接收状态设置为连接错误
							return;
						}
						can2_tp_value.TP_RX_MSG.tp_rx_msg.byte_count = len;					  // 数据长度
						can2_tp_value.TP_RX_MSG.packets_total = OneMessage.j1939_pid.Data[3]; // 数据包数
						can2_tp_value.TP_RX_MSG.time = J1939_TP_T2;							  // 连接管理超时时间
						can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_READ_DATA;				  // 设置接收状态为读取数据状态(DT)
					}
				}
				/*发送组帧*/
				else if (can2_tp_value.TP_Flag.state == J1939_TP_TX)
				{
					/*校验接收方PGN*/
					if (_pgn == can2_tp_value.TP_RX_MSG.tp_rx_msg.PGN)
					{
						switch (OneMessage.j1939_pid.Data[0])
						{
						case J1939_RTS_CONTROL_BYTE:
							/* 程序运行到这里，说明已经与网络中设备1建立虚拟链接（作为发送端），但是收到设备2的链接请求，并且同一个PGN消息请求*/
							/* 根据J1939-21数据链路层的规定，我们要保持原有的链接，不做任何事，设备2会应为超时自动放弃链接*/
							break;
						case J1939_CTS_CONTROL_BYTE:
							if ((can2_tp_value.TP_TX_MSG.state == J1939_TP_TX_CM_WAIT) || (can2_tp_value.TP_TX_MSG.state == J1939_TP_WAIT_ACK))
							{
								/*接收方能接收的包数*/
								if (OneMessage.j1939_pid.Data[1] == 0x00u)
								{
									/*刷新等待计数器*/
									can2_tp_value.TP_TX_MSG.tim = J1939_TP_T4;
								}
								else
								{
									/*请求数据包范围*/
									if ((OneMessage.j1939_pid.Data[2] + OneMessage.j1939_pid.Data[1]) > (can2_tp_value.TP_TX_MSG.packet_total + 1))
									{
										/*超出包范围，发送状态错误*/
										can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_ERROR;
									}
									else
									{
										/*响应请求参数*/
										can2_tp_value.TP_TX_MSG.packets_request_num = OneMessage.j1939_pid.Data[1];
										can2_tp_value.TP_TX_MSG.packet_offset_p = (j1939_uint8_t)(OneMessage.j1939_pid.Data[2] - 1);
										can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_DT;
									}
								}
							}
							break;
						case J1939_ACK_CONTROL_BYTE:
							if (can2_tp_value.TP_TX_MSG.state == J1939_TP_WAIT_ACK)
							{
								/*发送完成*/
								can2_tp_value.TP_TX_MSG.state = J1939_TX_DONE;
							}
							break;
						case J1939_ABORT_CONTROL_BYTE:
							// 收到放弃连接，等待协议主动放弃链接
							break;
						default:
							break;
						}
					}
				}
				/*单帧数据处理*/
				goto PutInReceiveQueue;
				break;
			default:
				/*单帧数据处理*/
				goto PutInReceiveQueue;
				break;
			}
		}
		/*数据传输*/
		else if (OneMessage.j1939_pid.PDUFormat == J1939_PF_DT)
		{
			/*CAN端口*/
			switch (port)
			{
			case Select_CAN_NODE_1:
				/*TP接收是否接收等待数据状态,源地址等于对应接收目标地址*/
				if ((can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_DATA_WAIT) && (can1_tp_value.TP_RX_MSG.tp_rx_msg.SA == OneMessage.j1939_pid.SourceAddress))
				{
					/*转存一包数据*/
					can1_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u] = OneMessage.j1939_pid.Data[1];
					can1_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 1] = OneMessage.j1939_pid.Data[2];
					can1_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 2] = OneMessage.j1939_pid.Data[3];
					can1_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 3] = OneMessage.j1939_pid.Data[4];
					can1_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 4] = OneMessage.j1939_pid.Data[5];
					can1_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 5] = OneMessage.j1939_pid.Data[6];
					can1_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 6] = OneMessage.j1939_pid.Data[7];
					/*特殊处理 重新接收已接收过的数据包*/
					if (OneMessage.j1939_pid.Data[0] > can1_tp_value.TP_RX_MSG.packets_ok_num)
					{
						/*计数接收包数*/
						can1_tp_value.TP_RX_MSG.packets_ok_num++;
					}
					can1_tp_value.TP_RX_MSG.time = J1939_TP_T1;
					/*是否接收完数据包*/
					if (can1_tp_value.TP_RX_MSG.packets_ok_num == can1_tp_value.TP_RX_MSG.packets_total)
					{
						/*接收状态置为读取数据*/
						can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_READ_DATA;
					}
				}
				break;
			case Select_CAN_NODE_2:
				/*TP接收是否接收等待数据状态,源地址等于对应接收目标地址*/
				if ((can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_DATA_WAIT) && (can2_tp_value.TP_RX_MSG.tp_rx_msg.SA == OneMessage.j1939_pid.SourceAddress))
				{
					/*转存一包数据*/
					can2_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u] = OneMessage.j1939_pid.Data[1];
					can2_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 1] = OneMessage.j1939_pid.Data[2];
					can2_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 2] = OneMessage.j1939_pid.Data[3];
					can2_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 3] = OneMessage.j1939_pid.Data[4];
					can2_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 4] = OneMessage.j1939_pid.Data[5];
					can2_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 5] = OneMessage.j1939_pid.Data[6];
					can2_tp_value.TP_RX_MSG.tp_rx_msg.data[(OneMessage.j1939_pid.Data[0] - 1) * 7u + 6] = OneMessage.j1939_pid.Data[7];
					/*特殊处理 重新接收已接收过的数据包*/
					if (OneMessage.j1939_pid.Data[0] > can2_tp_value.TP_RX_MSG.packets_ok_num)
					{
						/*计数接收包数*/
						can2_tp_value.TP_RX_MSG.packets_ok_num++;
					}
					can2_tp_value.TP_RX_MSG.time = J1939_TP_T1;
					/*是否接收完数据包*/
					if (can2_tp_value.TP_RX_MSG.packets_ok_num == can2_tp_value.TP_RX_MSG.packets_total)
					{
						/*接收状态置为读取数据*/
						can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_READ_DATA;
					}
				}
				break;
			default:
				break;
			}
		}
		else
		{
			/*单帧报文处理*/
		PutInReceiveQueue:
			{
				/*PDU格式1:0-239 	PDU格式2:240-255*/
				if (OneMessage.j1939_pid.PDUFormat < 240)
				{
					OneMessage.j1939_pid.PGN = (OneMessage.j1939_pid.Res << 17) +
						(OneMessage.j1939_pid.DataPage << 16) +
						(OneMessage.j1939_pid.PDUFormat << 8);
				}
				else
				{
					OneMessage.j1939_pid.PGN = (OneMessage.j1939_pid.Res << 17) +
						(OneMessage.j1939_pid.DataPage << 16) +
						(OneMessage.j1939_pid.PDUFormat << 8) +
						OneMessage.j1939_pid.PDUSpecific;
				}
				/*CAN端口*/
				switch (port)
				{
					/*CAN1*/
				case Select_CAN_NODE_1:
					if (can1_rx_queue.Queue_Count < J1939_QUEUE_SIZE)
					{
						// 队列数据数量累加
						can1_rx_queue.Queue_Count++;
						can1_rx_queue.Tail++;
						if (can1_rx_queue.Tail >= J1939_QUEUE_SIZE)
						{
							// 入队尾部计数清零
							can1_rx_queue.Tail = 0;
						}
					}
					/*报文消息入队*/
					can1_rx_queue.Queue[can1_rx_queue.Tail] = OneMessage;
					break;

					/*CAN2*/
				case Select_CAN_NODE_2:
					if (can2_rx_queue.Queue_Count < J1939_QUEUE_SIZE)
					{
						// 队列数据数量累加
						can2_rx_queue.Queue_Count++;
						can2_rx_queue.Tail++;
						if (can2_rx_queue.Tail >= J1939_QUEUE_SIZE)
						{
							// 入队尾部计数清零
							can2_rx_queue.Tail = 0;
						}
					}
					/*报文消息入队*/
					can2_rx_queue.Queue[can2_rx_queue.Tail] = OneMessage;
					break;

					/*CAN3*/
				case Select_CAN_NODE_3:
					if (can3_rx_queue.Queue_Count < J1939_QUEUE_SIZE)
					{
						// 队列数据数量累加
						can3_rx_queue.Queue_Count++;
						can3_rx_queue.Tail++;
						if (can3_rx_queue.Tail >= J1939_QUEUE_SIZE)
						{
							// 入队尾部计数清零
							can3_rx_queue.Tail = 0;
						}
					}
					/*报文消息入队*/
					can3_rx_queue.Queue[can3_rx_queue.Tail] = OneMessage;
					break;

					/*CAN4*/
				case Select_CAN_NODE_4:
					if (can4_rx_queue.Queue_Count < J1939_QUEUE_SIZE)
					{
						// 队列数据数量累加
						can4_rx_queue.Queue_Count++;
						can4_rx_queue.Tail++;
						if (can4_rx_queue.Tail >= J1939_QUEUE_SIZE)
						{
							// 入队尾部计数清零
							can4_rx_queue.Tail = 0;
						}
					}
					/*报文消息入队*/
					can4_rx_queue.Queue[can4_rx_queue.Tail] = OneMessage;
					break;

				default:
					break;
				}
			}
		}
	}
}

/**
 * @brief 发送报文消息
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 * @retval 0: 发送成功
 * @retval -1: 发送失败
 */
int J1939_TransmitMessage(CAN_NODE port)
{
	switch (port)
	{
	case Select_CAN_NODE_1:
		if (can1_tx_queue.Queue_Count == 0)
		{
			// 队列为空
			return RC_ERROR;
		}
		else
		{
			while (can1_tx_queue.Queue_Count > 0)
			{
				// 加载本机源地址
				can1_tx_queue.Queue[can1_tx_queue.Head].j1939_pid.SourceAddress = J1939_STARTING_ADDRESS_1;

				SendOneMessage(Select_CAN_NODE_1, (J1939_MESSAGE*)&can1_tx_queue.Queue[can1_tx_queue.Head]);
				can1_tx_queue.Head++;
				if (can1_tx_queue.Head >= J1939_QUEUE_SIZE)
					can1_tx_queue.Head = 0;
				can1_tx_queue.Queue_Count--;
			}
		}
		break;
	case Select_CAN_NODE_2:
		if (can2_tx_queue.Queue_Count == 0)
		{
			// 队列为空
			return RC_ERROR;
		}
		else
		{
			while (can2_tx_queue.Queue_Count > 0)
			{
				// 加载本机源地址
				can2_tx_queue.Queue[can2_tx_queue.Head].j1939_pid.SourceAddress = J1939_STARTING_ADDRESS_2;

				SendOneMessage(Select_CAN_NODE_2, (J1939_MESSAGE*)&can2_tx_queue.Queue[can2_tx_queue.Head]);
				can2_tx_queue.Head++;
				if (can2_tx_queue.Head >= J1939_QUEUE_SIZE)
					can2_tx_queue.Head = 0;
				can2_tx_queue.Queue_Count--;
			}
		}
		break;
	case Select_CAN_NODE_3:
		if (can3_tx_queue.Queue_Count == 0)
		{
			// 队列为空
			return RC_ERROR;
		}
		else
		{
			while (can3_tx_queue.Queue_Count > 0)
			{
				// 加载本机源地址
				can3_tx_queue.Queue[can3_tx_queue.Head].j1939_pid.SourceAddress = J1939_STARTING_ADDRESS_3;

				SendOneMessage(Select_CAN_NODE_3, (J1939_MESSAGE*)&can3_tx_queue.Queue[can3_tx_queue.Head]);
				can3_tx_queue.Head++;
				if (can3_tx_queue.Head >= J1939_QUEUE_SIZE)
					can3_tx_queue.Head = 0;
				can3_tx_queue.Queue_Count--;
			}
		}
		break;
	case Select_CAN_NODE_4:
		if (can4_tx_queue.Queue_Count == 0)
		{
			// 队列为空
			return RC_ERROR;
		}
		else
		{
			while (can4_tx_queue.Queue_Count > 0)
			{
				// 加载本机源地址
				can4_tx_queue.Queue[can4_tx_queue.Head].j1939_pid.SourceAddress = J1939_STARTING_ADDRESS_4;

				SendOneMessage(Select_CAN_NODE_4, (J1939_MESSAGE*)&can4_tx_queue.Queue[can4_tx_queue.Head]);
				can4_tx_queue.Head++;
				if (can4_tx_queue.Head >= J1939_QUEUE_SIZE)
					can4_tx_queue.Head = 0;
				can4_tx_queue.Queue_Count--;
			}
		}
		break;
	default:
		return RC_ERROR;
		break;
	}
	return RC_SUCCESS;
}

/**
 * @brief 1939消息出队（接收消息）
 * @param {CAN_NODE} port CAN端口
 * @param {J1939_MESSAGE*} pthis 消息对象
 * @return {*}
 * @retval 0: 出队成功
 * @retval 其他: 出队失败
 */
j1939_uint8_t J1939_DequeueMessage(CAN_NODE port, J1939_MESSAGE* pthis)
{
	j1939_uint8_t _rc = RC_SUCCESS;

	switch (port)
	{
	case Select_CAN_NODE_1:
		if (can1_rx_queue.Queue_Count == 0)
		{
			_rc = RC_QUEUEEMPTY;
		}
		else
		{
			*pthis = can1_rx_queue.Queue[can1_rx_queue.Head];
			can1_rx_queue.Head++;
			if (can1_rx_queue.Head > J1939_QUEUE_SIZE)
				can1_rx_queue.Head = 0;
			can1_rx_queue.Queue_Count--;
		}
		break;
	case Select_CAN_NODE_2:
		if (can2_rx_queue.Queue_Count == 0)
		{
			_rc = RC_QUEUEEMPTY;
		}
		else
		{
			*pthis = can2_rx_queue.Queue[can2_rx_queue.Head];
			can2_rx_queue.Head++;
			if (can2_rx_queue.Head > J1939_QUEUE_SIZE)
				can2_rx_queue.Head = 0;
			can2_rx_queue.Queue_Count--;
		}
		break;
	case Select_CAN_NODE_3:
		if (can3_rx_queue.Queue_Count == 0)
		{
			_rc = RC_QUEUEEMPTY;
		}
		else
		{
			*pthis = can3_rx_queue.Queue[can3_rx_queue.Head];
			can3_rx_queue.Head++;
			if (can3_rx_queue.Head > J1939_QUEUE_SIZE)
				can3_rx_queue.Head = 0;
			can3_rx_queue.Queue_Count--;
		}
		break;
	case Select_CAN_NODE_4:
		if (can4_rx_queue.Queue_Count == 0)
		{
			_rc = RC_QUEUEEMPTY;
		}
		else
		{
			*pthis = can4_rx_queue.Queue[can4_rx_queue.Head];
			can4_rx_queue.Head++;
			if (can4_rx_queue.Head > J1939_QUEUE_SIZE)
				can4_rx_queue.Head = 0;
			can4_rx_queue.Queue_Count--;
		}
		break;
	default:
		_rc = RC_PARAMERROR;
		break;
	}
	return _rc;
}

/**
 * @brief J1939消息入队（发送消息）
 * @param {CAN_NODE} port CAN端口
 * @param {J1939_MESSAGE} *pthis 消息对象
 * @return {*}
 * @retval 0: 入队成功
 * @retval 其他: 入队失败
 */
j1939_uint8_t J1939_EnqueueMessage(CAN_NODE port, J1939_MESSAGE* pthis)
{
	j1939_uint8_t _rc = RC_SUCCESS;

	switch (port)
	{
	case Select_CAN_NODE_1:
		if (can1_tx_queue.Queue_Count < J1939_QUEUE_SIZE)
		{
			can1_tx_queue.Queue_Count++;
			can1_tx_queue.Tail++;
			if (can1_tx_queue.Tail >= J1939_QUEUE_SIZE)
				can1_tx_queue.Tail = 0;
		}
		can1_tx_queue.Queue[can1_tx_queue.Tail] = *pthis;
		break;
	case Select_CAN_NODE_2:
		if (can2_tx_queue.Queue_Count < J1939_QUEUE_SIZE)
		{
			can2_tx_queue.Queue_Count++;
			can2_tx_queue.Tail++;
			if (can2_tx_queue.Tail >= J1939_QUEUE_SIZE)
				can2_tx_queue.Tail = 0;
		}
		can2_tx_queue.Queue[can2_tx_queue.Tail] = *pthis;
		break;
	case Select_CAN_NODE_3:
		if (can3_tx_queue.Queue_Count < J1939_QUEUE_SIZE)
		{
			can3_tx_queue.Queue_Count++;
			can3_tx_queue.Tail++;
			if (can3_tx_queue.Tail >= J1939_QUEUE_SIZE)
				can3_tx_queue.Tail = 0;
		}
		can3_tx_queue.Queue[can3_tx_queue.Tail] = *pthis;
		break;
	case Select_CAN_NODE_4:
		if (can4_tx_queue.Queue_Count < J1939_QUEUE_SIZE)
		{
			can4_tx_queue.Queue_Count++;
			can4_tx_queue.Tail++;
			if (can4_tx_queue.Tail >= J1939_QUEUE_SIZE)
				can4_tx_queue.Tail = 0;
		}
		can4_tx_queue.Queue[can4_tx_queue.Tail] = *pthis;
		break;
	default:
		_rc = RC_PARAMERROR;
		break;
	}
	return _rc;
}

/**
 * @brief 发送TP.CM_RTS 链接管理请求
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 */
void J1939_CM_Start(CAN_NODE port)
{
	j1939_uint32_t pgn_num;
	J1939_MESSAGE _msg;

	switch (port)
	{
	case Select_CAN_NODE_1:
		pgn_num = can1_tp_value.TP_TX_MSG.tp_tx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.SourceAddress = can1_tp_value.TP_TX_MSG.tp_tx_msg.SA;
		_msg.j1939_pid.DataLength = 8;
		_msg.j1939_pid.Data[0] = J1939_RTS_CONTROL_BYTE;
		_msg.j1939_pid.Data[1] = (j1939_uint8_t)can1_tp_value.TP_TX_MSG.tp_tx_msg.byte_count;
		_msg.j1939_pid.Data[2] = (j1939_uint8_t)(can1_tp_value.TP_TX_MSG.tp_tx_msg.byte_count >> 8);
		_msg.j1939_pid.Data[3] = can1_tp_value.TP_TX_MSG.packet_total;
		_msg.j1939_pid.Data[4] = 0XFF;
		_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
		_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
		_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

		J1939_EnqueueMessage(Select_CAN_NODE_1, &_msg);

		/*刷新等待时间，触发下一个步骤*/
		can1_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
		can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_CM_WAIT;
		break;
	case Select_CAN_NODE_2:
		pgn_num = can2_tp_value.TP_TX_MSG.tp_tx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.SourceAddress = can2_tp_value.TP_TX_MSG.tp_tx_msg.SA;
		_msg.j1939_pid.DataLength = 8;
		_msg.j1939_pid.Data[0] = J1939_RTS_CONTROL_BYTE;
		_msg.j1939_pid.Data[1] = (j1939_uint8_t)can2_tp_value.TP_TX_MSG.tp_tx_msg.byte_count;
		_msg.j1939_pid.Data[2] = (j1939_uint8_t)(can2_tp_value.TP_TX_MSG.tp_tx_msg.byte_count >> 8);
		_msg.j1939_pid.Data[3] = can2_tp_value.TP_TX_MSG.packet_total;
		_msg.j1939_pid.Data[4] = 0XFF;
		_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
		_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
		_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

		J1939_EnqueueMessage(Select_CAN_NODE_2, &_msg);

		/*刷新等待时间，触发下一个步骤*/
		can2_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
		can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_CM_WAIT;
		break;
	default:
		break;
	}
}

/**
 * @brief 发送TP.CM_CTS 或 EndofMsgAck 准备发送和消息结束应答
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 */
void J1939_Read_DT_Packet(CAN_NODE port)
{
	j1939_uint32_t pgn_num;
	J1939_MESSAGE _msg;

	switch (port)
	{
	case Select_CAN_NODE_1:

		pgn_num = can1_tp_value.TP_RX_MSG.tp_rx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.PDUSpecific = can1_tp_value.TP_RX_MSG.tp_rx_msg.SA;
		_msg.j1939_pid.DataLength = 8;

		if (can1_tp_value.TP_RX_MSG.packets_total > can1_tp_value.TP_RX_MSG.packets_ok_num)
		{
			_msg.j1939_pid.Data[0] = J1939_CTS_CONTROL_BYTE;
			_msg.j1939_pid.Data[1] = can1_tp_value.TP_RX_MSG.packets_total;
			_msg.j1939_pid.Data[2] = can1_tp_value.TP_RX_MSG.packets_ok_num + 1;
			_msg.j1939_pid.Data[3] = 0XFF;
			_msg.j1939_pid.Data[4] = 0XFF;
			_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
			_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
			_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

			J1939_EnqueueMessage(Select_CAN_NODE_1, &_msg);
			can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_DATA_WAIT;
		}
		else
		{
			_msg.j1939_pid.Data[0] = J1939_ACK_CONTROL_BYTE;
			_msg.j1939_pid.Data[1] = can1_tp_value.TP_RX_MSG.tp_rx_msg.byte_count & 0xff;
			_msg.j1939_pid.Data[2] = (can1_tp_value.TP_RX_MSG.tp_rx_msg.byte_count >> 8) & 0xff;
			_msg.j1939_pid.Data[3] = can1_tp_value.TP_RX_MSG.packets_total;
			_msg.j1939_pid.Data[4] = 0XFF;
			_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
			_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
			_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

			J1939_EnqueueMessage(Select_CAN_NODE_1, &_msg);
			can1_tp_value.TP_RX_MSG.state = J1939_RX_DONE;
		}
		break;
	case Select_CAN_NODE_2:

		pgn_num = can2_tp_value.TP_RX_MSG.tp_rx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.PDUSpecific = can2_tp_value.TP_RX_MSG.tp_rx_msg.SA;
		_msg.j1939_pid.DataLength = 8;

		if (can2_tp_value.TP_RX_MSG.packets_total > can2_tp_value.TP_RX_MSG.packets_ok_num)
		{
			_msg.j1939_pid.Data[0] = J1939_CTS_CONTROL_BYTE;
			_msg.j1939_pid.Data[1] = can2_tp_value.TP_RX_MSG.packets_total;
			_msg.j1939_pid.Data[2] = can2_tp_value.TP_RX_MSG.packets_ok_num + 1;
			_msg.j1939_pid.Data[3] = 0XFF;
			_msg.j1939_pid.Data[4] = 0XFF;
			_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
			_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
			_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

			J1939_EnqueueMessage(Select_CAN_NODE_2, &_msg);
			can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_DATA_WAIT;
		}
		else
		{
			_msg.j1939_pid.Data[0] = J1939_ACK_CONTROL_BYTE;
			_msg.j1939_pid.Data[1] = can2_tp_value.TP_RX_MSG.tp_rx_msg.byte_count & 0xff;
			_msg.j1939_pid.Data[2] = (can2_tp_value.TP_RX_MSG.tp_rx_msg.byte_count >> 8) & 0xff;
			_msg.j1939_pid.Data[3] = can2_tp_value.TP_RX_MSG.packets_total;
			_msg.j1939_pid.Data[4] = 0XFF;
			_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
			_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
			_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

			J1939_EnqueueMessage(Select_CAN_NODE_2, &_msg);
			can2_tp_value.TP_RX_MSG.state = J1939_RX_DONE;
		}
		break;
	default:
		break;
	}
}

/**
 * @brief 发送TP.DT (传输数据)
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 */
void J1939_TP_DT_Packet_Send(CAN_NODE port)
{
	j1939_uint16_t _packet_offset_p;
	j1939_int32_t _i = 0;
	J1939_MESSAGE _msg;

	switch (port)
	{
	case Select_CAN_NODE_1:
		if (can1_tp_value.TP_TX_MSG.packets_request_num > 0)
		{
			can1_tp_value.TP_TX_MSG.packets_request_num--;
			/*获取数据偏移指针、加载数据包编号*/
			_packet_offset_p = (j1939_uint16_t)(can1_tp_value.TP_TX_MSG.packet_offset_p * 7u);
			_msg.j1939_pid.Data[0] = (j1939_uint8_t)(can1_tp_value.TP_TX_MSG.packet_offset_p + 1u);

			for (_i = 0; _i < 7; _i++)
			{
				_msg.j1939_pid.Data[_i + 1] = can1_tp_value.TP_TX_MSG.tp_tx_msg.data[_packet_offset_p + _i];
			}
			/*确认是否时最后一包数据*/
			if (can1_tp_value.TP_TX_MSG.packet_offset_p == (can1_tp_value.TP_TX_MSG.packet_total - 1u))
			{
				/*参数群是否能填满、是否需要填充*/
				if (_packet_offset_p > can1_tp_value.TP_TX_MSG.tp_tx_msg.byte_count - 7)
				{
					/*计算填充数据个数*/
					_i = can1_tp_value.TP_TX_MSG.tp_tx_msg.byte_count - _packet_offset_p - 7;

					for (; _i < 0; _i++)
					{
						/*默认J1939参数群大小为8*/
						_msg.j1939_pid.Data[_i + 8] = 0xFF;
					}
				}

				can1_tp_value.TP_TX_MSG.packets_request_num = 0;
				can1_tp_value.TP_TX_MSG.packet_offset_p = 0;
				can1_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
				/*跳转步骤，等待结束确认或重新发送数据请求*/
				can1_tp_value.TP_TX_MSG.state = J1939_TP_WAIT_ACK;
			}
			else
			{
				/*准备下一个数据包发送*/
				can1_tp_value.TP_TX_MSG.packet_offset_p++;
				can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_DT;
			}

			/*消息入队*/
			J1939_EnqueueMessage(Select_CAN_NODE_1, &_msg);
		}
		else
		{
			can1_tp_value.TP_TX_MSG.packets_request_num = 0;
			can1_tp_value.TP_TX_MSG.packet_offset_p = 0;
			can1_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
			can1_tp_value.TP_TX_MSG.state = J1939_TP_WAIT_ACK;
		}
		break;
	case Select_CAN_NODE_2:
		if (can2_tp_value.TP_TX_MSG.packets_request_num > 0)
		{
			can2_tp_value.TP_TX_MSG.packets_request_num--;
			/*获取数据偏移指针、加载数据包编号*/
			_packet_offset_p = (j1939_uint16_t)(can2_tp_value.TP_TX_MSG.packet_offset_p * 7u);
			_msg.j1939_pid.Data[0] = (j1939_uint8_t)(can2_tp_value.TP_TX_MSG.packet_offset_p + 1u);

			for (_i = 0; _i < 7; _i++)
			{
				_msg.j1939_pid.Data[_i + 1] = can2_tp_value.TP_TX_MSG.tp_tx_msg.data[_packet_offset_p + _i];
			}
			/*确认是否时最后一包数据*/
			if (can2_tp_value.TP_TX_MSG.packet_offset_p == (can2_tp_value.TP_TX_MSG.packet_total - 1u))
			{
				/*参数群是否能填满、是否需要填充*/
				if (_packet_offset_p > can2_tp_value.TP_TX_MSG.tp_tx_msg.byte_count - 7)
				{
					/*计算填充数据个数*/
					_i = can2_tp_value.TP_TX_MSG.tp_tx_msg.byte_count - _packet_offset_p - 7;

					for (; _i < 0; _i++)
					{
						/*默认J1939参数群大小为8*/
						_msg.j1939_pid.Data[_i + 8] = 0xFF;
					}
				}

				can2_tp_value.TP_TX_MSG.packets_request_num = 0;
				can2_tp_value.TP_TX_MSG.packet_offset_p = 0;
				can2_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
				/*跳转步骤，等待结束确认或重新发送数据请求*/
				can2_tp_value.TP_TX_MSG.state = J1939_TP_WAIT_ACK;
			}
			else
			{
				/*准备下一个数据包发送*/
				can2_tp_value.TP_TX_MSG.packet_offset_p++;
				can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_DT;
			}

			/*消息入队*/
			J1939_EnqueueMessage(Select_CAN_NODE_2, &_msg);
		}
		else
		{
			can2_tp_value.TP_TX_MSG.packets_request_num = 0;
			can2_tp_value.TP_TX_MSG.packet_offset_p = 0;
			can2_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
			can2_tp_value.TP_TX_MSG.state = J1939_TP_WAIT_ACK;
		}
		break;
	default:
		break;
	}
}

/**
 * @brief 中断TP链接(作为发送端时)
 * @param {CAN_NODE} port CAN 端口
 * @return {*}
 */
void J1939_TP_TX_Abort(CAN_NODE port)
{
	j1939_uint32_t pgn_num;
	J1939_MESSAGE _msg;

	switch (port)
	{
	case Select_CAN_NODE_1:
		pgn_num = can1_tp_value.TP_TX_MSG.tp_tx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.SourceAddress = can1_tp_value.TP_TX_MSG.tp_tx_msg.SA;
		_msg.j1939_pid.DataLength = 8;

		_msg.j1939_pid.Data[0] = J1939_ABORT_CONTROL_BYTE;
		_msg.j1939_pid.Data[1] = 0xFF;
		_msg.j1939_pid.Data[2] = 0xFF;
		_msg.j1939_pid.Data[3] = 0xFF;
		_msg.j1939_pid.Data[4] = 0xFF;
		_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
		_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
		_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

		J1939_EnqueueMessage(Select_CAN_NODE_1, &_msg);
		/*结束发送*/
		can1_tp_value.TP_TX_MSG.state = J1939_TX_DONE;
		break;
	case Select_CAN_NODE_2:
		pgn_num = can2_tp_value.TP_TX_MSG.tp_tx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.SourceAddress = can2_tp_value.TP_TX_MSG.tp_tx_msg.SA;
		_msg.j1939_pid.DataLength = 8;

		_msg.j1939_pid.Data[0] = J1939_ABORT_CONTROL_BYTE;
		_msg.j1939_pid.Data[1] = 0xFF;
		_msg.j1939_pid.Data[2] = 0xFF;
		_msg.j1939_pid.Data[3] = 0xFF;
		_msg.j1939_pid.Data[4] = 0xFF;
		_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
		_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
		_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

		J1939_EnqueueMessage(Select_CAN_NODE_2, &_msg);
		/*结束发送*/
		can2_tp_value.TP_TX_MSG.state = J1939_TX_DONE;
		break;
	default:
		break;
	}
}

/**
 * @brief 中断TP链接(作为接收端时)
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 */
void J1939_TP_RX_Abort(CAN_NODE port)
{
	j1939_uint32_t pgn_num;
	J1939_MESSAGE _msg;

	switch (port)
	{
	case Select_CAN_NODE_1:
		pgn_num = can1_tp_value.TP_RX_MSG.tp_rx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.SourceAddress = can1_tp_value.TP_RX_MSG.tp_rx_msg.SA;
		_msg.j1939_pid.DataLength = 8;

		_msg.j1939_pid.Data[0] = J1939_ABORT_CONTROL_BYTE;
		_msg.j1939_pid.Data[1] = 0xFF;
		_msg.j1939_pid.Data[2] = 0xFF;
		_msg.j1939_pid.Data[3] = 0xFF;
		_msg.j1939_pid.Data[4] = 0xFF;
		_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
		_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
		_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

		J1939_EnqueueMessage(Select_CAN_NODE_1, &_msg);
		/*结束发送*/
		can1_tp_value.TP_RX_MSG.state = J1939_RX_DONE;
		break;
	case Select_CAN_NODE_2:
		pgn_num = can2_tp_value.TP_RX_MSG.tp_rx_msg.PGN;

		_msg.j1939_pid.Priority = J1939_TP_CM_PRIORITY;
		_msg.j1939_pid.DataPage = 0;
		_msg.j1939_pid.PDUFormat = J1939_PF_TP_CM;
		_msg.j1939_pid.SourceAddress = can2_tp_value.TP_RX_MSG.tp_rx_msg.SA;
		_msg.j1939_pid.DataLength = 8;

		_msg.j1939_pid.Data[0] = J1939_ABORT_CONTROL_BYTE;
		_msg.j1939_pid.Data[1] = 0xFF;
		_msg.j1939_pid.Data[2] = 0xFF;
		_msg.j1939_pid.Data[3] = 0xFF;
		_msg.j1939_pid.Data[4] = 0xFF;
		_msg.j1939_pid.Data[5] = (j1939_uint8_t)pgn_num & 0xFF;
		_msg.j1939_pid.Data[6] = (j1939_uint8_t)(pgn_num >> 8) & 0xFF;
		_msg.j1939_pid.Data[7] = (j1939_uint8_t)(pgn_num >> 16) & 0xFF;

		J1939_EnqueueMessage(Select_CAN_NODE_2, &_msg);
		/*结束发送*/
		can2_tp_value.TP_RX_MSG.state = J1939_RX_DONE;
		break;
	default:
		break;
	}
}

/**
 * @brief TP协议超时计时器
 * @param {j1939_uint16_t} dt_ms 单位时间
 * @param {TP_TEMP_Value*} pthis TP对象
 * @return {*}
 * @retval 0: 未超时
 * @retval 1: 超时
 */
j1939_uint8_t J1939_TP_RefreshCMTimer(j1939_uint16_t dt_ms, TP_TEMP_Value* pthis)
{
	if (pthis->TP_Flag.state == J1939_TP_TX)
	{
		if ((pthis->TP_TX_MSG.state == J1939_TP_TX_CM_WAIT) || (pthis->TP_TX_MSG.state == J1939_TP_WAIT_ACK))
		{
			if (pthis->TP_TX_MSG.tim > dt_ms)
			{
				pthis->TP_TX_MSG.tim = pthis->TP_TX_MSG.tim - dt_ms;
				return J1939_TP_TIMEOUT_NORMAL;
			}
			else
			{
				pthis->TP_TX_MSG.tim = 0u;
				return J1939_TP_TIMEOUT_ABNORMAL;
			}
		}
		else
		{
			return J1939_TP_TIMEOUT_NORMAL;
		}
	}
	else if (pthis->TP_Flag.state == J1939_TP_RX)
	{
		if (pthis->TP_RX_MSG.state == J1939_TP_RX_DATA_WAIT)
		{
			if (pthis->TP_RX_MSG.time > dt_ms)
			{
				pthis->TP_RX_MSG.time = pthis->TP_RX_MSG.time - dt_ms;
				return J1939_TP_TIMEOUT_NORMAL;
			}
			else
			{
				pthis->TP_RX_MSG.time = 0u;
				return J1939_TP_TIMEOUT_ABNORMAL;
			}
		}
		else
		{
			return J1939_TP_TIMEOUT_NORMAL;
		}
	}
	else
	{
		return J1939_TP_TIMEOUT_NORMAL;
	}
}

/**
 * @brief TP协议心跳
 * @param {CAN_NODE} port CAN端口
 * @param {j1939_uint16_t} dt_ms 心跳周期时间
 * @return {*}
 */
void J1939_TP_Poll(CAN_NODE port, j1939_uint16_t dt_ms)
{
	if (port == Select_CAN_NODE_1)
	{
		if (can1_tp_value.TP_Flag.state == J1939_TP_NULL || can1_tp_value.TP_Flag.state == J1939_TP_OSBUSY)
		{
			return;
		}
		if (can1_tp_value.TP_Flag.state == J1939_TP_RX)
		{
			switch (can1_tp_value.TP_RX_MSG.state)
			{
			case J1939_TP_RX_WAIT:;
				break;
			case J1939_TP_RX_READ_DATA:
				/*发送应答消息*/
				J1939_Read_DT_Packet(Select_CAN_NODE_1);
				break;
			case J1939_TP_RX_DATA_WAIT:
				/*等待TP.DT传输消息*/
				if (J1939_TP_RefreshCMTimer(dt_ms, &can1_tp_value) == J1939_TP_TIMEOUT_ABNORMAL)
				{
					/*等待超时，跳转异常步骤*/
					can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_ERROR;
				}
				break;
			case J1939_TP_RX_ERROR:
				/*中断链接*/
				J1939_TP_RX_Abort(Select_CAN_NODE_1);
				break;
			case J1939_RX_DONE:
				can1_tp_value.TP_RX_MSG.packets_ok_num = 0;
				can1_tp_value.TP_RX_MSG.packets_total = 0;
				can1_tp_value.TP_RX_MSG.time = J1939_TP_T3;
				can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_WAIT;
				can1_tp_value.TP_Flag.state = J1939_TP_NULL;
				break;
			default:
				break;
			}
		}
		else if (can1_tp_value.TP_Flag.state == J1939_TP_TX)
		{
			switch (can1_tp_value.TP_TX_MSG.state)
			{
			case J1939_TP_TX_WAIT:
				break;
			case J1939_TP_TX_CM_START:
				/*发送TP.CM_RTS请求链接消息*/
				J1939_CM_Start(Select_CAN_NODE_1);
				break;
			case J1939_TP_TX_CM_WAIT:
				/*等待接收TP.CM_CTS链接应答消息*/
				if (J1939_TP_RefreshCMTimer(dt_ms, &can1_tp_value) == J1939_TP_TIMEOUT_ABNORMAL)
				{
					/*等待超时，跳转异常步骤*/
					can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_ERROR;
				}
				break;
			case J1939_TP_TX_DT:
				J1939_TP_DT_Packet_Send(Select_CAN_NODE_1);
				break;
			case J1939_TP_WAIT_ACK:
				/*等待接收TP.CM_EndofMsgACK传输结束消息*/
				if (J1939_TP_RefreshCMTimer(dt_ms, &can1_tp_value) == J1939_TP_TIMEOUT_ABNORMAL)
				{
					/*等待超时，跳转异常步骤*/
					can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_ERROR;
				}
				break;
			case J1939_TP_TX_ERROR:
				/*中断链接*/
				J1939_TP_TX_Abort(Select_CAN_NODE_1);
				break;
			case J1939_TX_DONE:
				can1_tp_value.TP_TX_MSG.packets_request_num = 0;
				can1_tp_value.TP_TX_MSG.packet_offset_p = 0;
				can1_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
				can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_WAIT;
				can1_tp_value.TP_Flag.state = J1939_TP_NULL;
				break;
			default:
				break;
			}
		}
	}
	else if (port == Select_CAN_NODE_2)
	{
		if (can2_tp_value.TP_Flag.state == J1939_TP_NULL || can2_tp_value.TP_Flag.state == J1939_TP_OSBUSY)
		{
			return;
		}
		if (can2_tp_value.TP_Flag.state == J1939_TP_RX)
		{
			switch (can2_tp_value.TP_RX_MSG.state)
			{
			case J1939_TP_RX_WAIT:;
				break;
			case J1939_TP_RX_READ_DATA:
				/*发送应答消息*/
				J1939_Read_DT_Packet(Select_CAN_NODE_2);
				break;
			case J1939_TP_RX_DATA_WAIT:
				/*等待TP.DT传输消息*/
				if (J1939_TP_RefreshCMTimer(dt_ms, &can2_tp_value) == J1939_TP_TIMEOUT_ABNORMAL)
				{
					/*等待超时，跳转异常步骤*/
					can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_ERROR;
				}
				break;
			case J1939_TP_RX_ERROR:
				/*中断链接*/
				J1939_TP_RX_Abort(Select_CAN_NODE_2);
				break;
			case J1939_RX_DONE:
				can2_tp_value.TP_RX_MSG.packets_ok_num = 0;
				can2_tp_value.TP_RX_MSG.packets_total = 0;
				can2_tp_value.TP_RX_MSG.time = J1939_TP_T3;
				can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_WAIT;
				can2_tp_value.TP_Flag.state = J1939_TP_NULL;
				break;
			default:
				break;
			}
		}
		else if (can2_tp_value.TP_Flag.state == J1939_TP_TX)
		{
			switch (can2_tp_value.TP_TX_MSG.state)
			{
			case J1939_TP_TX_WAIT:
				break;
			case J1939_TP_TX_CM_START:
				/*发送TP.CM_RTS请求链接消息*/
				J1939_CM_Start(Select_CAN_NODE_2);
				break;
			case J1939_TP_TX_CM_WAIT:
				/*等待接收TP.CM_CTS链接应答消息*/
				if (J1939_TP_RefreshCMTimer(dt_ms, &can2_tp_value) == J1939_TP_TIMEOUT_ABNORMAL)
				{
					/*等待超时，跳转异常步骤*/
					can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_ERROR;
				}
				break;
			case J1939_TP_TX_DT:
				J1939_TP_DT_Packet_Send(Select_CAN_NODE_2);
				break;
			case J1939_TP_WAIT_ACK:
				/*等待接收TP.CM_EndofMsgACK传输结束消息*/
				if (J1939_TP_RefreshCMTimer(dt_ms, &can2_tp_value) == J1939_TP_TIMEOUT_ABNORMAL)
				{
					/*等待超时，跳转异常步骤*/
					can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_ERROR;
				}
				break;
			case J1939_TP_TX_ERROR:
				/*中断链接*/
				J1939_TP_TX_Abort(Select_CAN_NODE_2);
				break;
			case J1939_TX_DONE:
				can2_tp_value.TP_TX_MSG.packets_request_num = 0;
				can2_tp_value.TP_TX_MSG.packet_offset_p = 0;
				can2_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
				can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_WAIT;
				can2_tp_value.TP_Flag.state = J1939_TP_NULL;
				break;
			default:
				break;
			}
		}
	}
}

/**
 * @brief 轮询报文消息发送
 * @return {*}
 */
void J1939_Poll(void)
{
	J1939_TransmitMessage(Select_CAN_NODE_1);
	J1939_TransmitMessage(Select_CAN_NODE_2);
	J1939_TransmitMessage(Select_CAN_NODE_3);
	J1939_TransmitMessage(Select_CAN_NODE_4);
}

/**
 * @brief 中断读取数据
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 */
void J1939_ISR(CAN_NODE port)
{
	switch (port)
	{
	case Select_CAN_NODE_1:
		J1939_ReceiveMessage(Select_CAN_NODE_1);
		break;
	case Select_CAN_NODE_2:
		J1939_ReceiveMessage(Select_CAN_NODE_2);
		break;
	case Select_CAN_NODE_3:
		J1939_ReceiveMessage(Select_CAN_NODE_3);
		break;
	case Select_CAN_NODE_4:
		J1939_ReceiveMessage(Select_CAN_NODE_4);
		break;
	default:
		break;
	}
}

/**
 * @brief J1939协议栈初始化
 * @return {*}
 */
void J1939_Init(void)
{
	/*发送队列初始化*/
	can1_tx_queue.Head = 0;
	can2_tx_queue.Head = 0;
	can3_tx_queue.Head = 0;
	can4_tx_queue.Head = 0;
	can1_tx_queue.Tail = 0xFF;
	can2_tx_queue.Tail = 0xFF;
	can3_tx_queue.Tail = 0xFF;
	can4_tx_queue.Tail = 0xFF;
	can1_tx_queue.Queue_Count = 0;
	can2_tx_queue.Queue_Count = 0;
	can3_tx_queue.Queue_Count = 0;
	can4_tx_queue.Queue_Count = 0;
	/*接收队列初始化*/
	can1_rx_queue.Head = 0;
	can2_rx_queue.Head = 0;
	can3_rx_queue.Head = 0;
	can4_rx_queue.Head = 0;
	can1_rx_queue.Tail = 0xFF;
	can2_rx_queue.Tail = 0xFF;
	can3_rx_queue.Tail = 0xFF;
	can4_rx_queue.Tail = 0xFF;
	can1_rx_queue.Queue_Count = 0;
	can2_rx_queue.Queue_Count = 0;
	can3_rx_queue.Queue_Count = 0;
	can4_rx_queue.Queue_Count = 0;
	/*TP协议初始化*/
	can1_tp_value.TP_Flag.state = J1939_TP_NULL;
	can2_tp_value.TP_Flag.state = J1939_TP_NULL;

	/*组帧发送*/
	can1_tp_value.TP_TX_MSG.packets_request_num = 0;
	can1_tp_value.TP_TX_MSG.packet_total = 0;
	can1_tp_value.TP_TX_MSG.tim = 0;
	can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_WAIT;
	can2_tp_value.TP_TX_MSG.packets_request_num = 0;
	can2_tp_value.TP_TX_MSG.packet_total = 0;
	can2_tp_value.TP_TX_MSG.tim = 0;
	can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_WAIT;

	/*组帧接收*/
	can1_tp_value.TP_RX_MSG.packets_ok_num = 0;
	can1_tp_value.TP_RX_MSG.packets_total = 0;
	can1_tp_value.TP_RX_MSG.time = 0;
	can1_tp_value.TP_RX_MSG.state = J1939_TP_RX_WAIT;
	can2_tp_value.TP_RX_MSG.packets_ok_num = 0;
	can2_tp_value.TP_RX_MSG.packets_total = 0;
	can2_tp_value.TP_RX_MSG.time = 0;
	can2_tp_value.TP_RX_MSG.state = J1939_TP_RX_WAIT;

}

    /**
     * @brief J1939单帧消息发送
     * @param {J1939_MESSAGE*} pthis 消息对象
     * @param {CAN_NODE} port CAN端口
     * @return {*}
     * @retval 0: 入队成功
     * @retval 其他: 入队失败
     */
    j1939_uint8_t J1939_Send_Message(J1939_MESSAGE* pthis, CAN_NODE port)
    {
        return J1939_EnqueueMessage(port, pthis);
    }

/**
 * @brief J1939单帧消息读取
 * @param {J1939_MESSAGE*} pthis 消息缓存区
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 * @retval 0: 出队成功
 * @retval 其他: 出队失败
 */
j1939_uint8_t J1939_Read_Message(J1939_MESSAGE* pthis, CAN_NODE port)
{
	return J1939_DequeueMessage(port, pthis);
}

/**
 * @brief 组帧TP消息发送
 * @param {j1939_uint32_t} PGN 参数群编号
 * @param {j1939_uint8_t} SA 目标地址
 * @param {j1939_uint8_t*} data 数据缓存地址
 * @param {j1939_uint16_t} data_num 数据大小
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 * @retval 0: 入队成功
 * @retval 其他: 入队失败
 */
j1939_uint8_t J1939_TP_TX_Message(j1939_uint32_t PGN, j1939_uint8_t SA, j1939_uint8_t* data, j1939_uint16_t data_num, CAN_NODE port)
{
	j1939_uint16_t _byte_count = 0;

	switch (port)
	{
	case Select_CAN_NODE_1:
		/*取得发送权限*/
		if (can1_tp_value.TP_Flag.state == J1939_TP_NULL)
		{
			can1_tp_value.TP_Flag.state = J1939_TP_TX;
		}
		else
		{
			return RC_ERROR;
		}

		can1_tp_value.TP_TX_MSG.tp_tx_msg.PGN = PGN;
		can1_tp_value.TP_TX_MSG.tp_tx_msg.SA = SA;
		can1_tp_value.TP_TX_MSG.tp_tx_msg.byte_count = data_num;
		for (_byte_count = 0; _byte_count < data_num; _byte_count++)
		{
			can1_tp_value.TP_TX_MSG.tp_tx_msg.data[_byte_count] = data[_byte_count];
		}
		can1_tp_value.TP_TX_MSG.packet_offset_p = 0;
		can1_tp_value.TP_TX_MSG.packets_request_num = 0;
		can1_tp_value.TP_TX_MSG.packet_total = data_num / 7;
		if ((data_num % 7) != 0)
		{
			can1_tp_value.TP_TX_MSG.packet_total++;
		}
		can1_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
		/*触发TP.CM_START*/
		can1_tp_value.TP_TX_MSG.state = J1939_TP_TX_CM_START;
		return RC_SUCCESS;
		break;

	case Select_CAN_NODE_2:
		/*取得发送权限*/
		if (can2_tp_value.TP_Flag.state == J1939_TP_NULL)
		{
			can2_tp_value.TP_Flag.state = J1939_TP_TX;
		}
		else
		{
			return RC_ERROR;
		}

		can2_tp_value.TP_TX_MSG.tp_tx_msg.PGN = PGN;
		can2_tp_value.TP_TX_MSG.tp_tx_msg.SA = SA;
		can2_tp_value.TP_TX_MSG.tp_tx_msg.byte_count = data_num;
		for (_byte_count = 0; _byte_count < data_num; _byte_count++)
		{
			can2_tp_value.TP_TX_MSG.tp_tx_msg.data[_byte_count] = data[_byte_count];
		}
		can2_tp_value.TP_TX_MSG.packet_offset_p = 0;
		can2_tp_value.TP_TX_MSG.packets_request_num = 0;
		can2_tp_value.TP_TX_MSG.packet_total = data_num / 7;
		if ((data_num % 7) != 0)
		{
			can2_tp_value.TP_TX_MSG.packet_total++;
		}
		can2_tp_value.TP_TX_MSG.tim = J1939_TP_T3;
		/*触发TP.CM_START*/
		can2_tp_value.TP_TX_MSG.state = J1939_TP_TX_CM_START;
		return RC_SUCCESS;
		break;
	default:
		return RC_PARAMERROR;
		break;
	}
}

/**
 * @brief 组帧TP消息读取
 * @param {TP_RX_MESSAGE*} pthis 消息缓存区
 * @param {CAN_NODE} port CAN端口
 * @return {*}
 * @retval 0: 出队成功
 * @retval 其他:出队失败
 */
j1939_uint8_t J1939_TP_RX_Message(TP_RX_MESSAGE* pthis, CAN_NODE port)
{
	j1939_uint16_t _byte_count = 0;

	switch (port)
	{
	case Select_CAN_NODE_1:
		/*能否获取数据*/
		if (can1_tp_value.TP_Flag.state != J1939_TP_NULL || can1_tp_value.TP_RX_MSG.tp_rx_msg.PGN == 0)
		{
			return RC_ERROR;
		} /*缓存区大小是否够*/
		if ((pthis->data_num) < can1_tp_value.TP_RX_MSG.tp_rx_msg.byte_count)
		{
			return RC_ERROR;
		}

		/*转存数据*/
		for (_byte_count = 0; _byte_count < pthis->data_num; _byte_count++)
		{
			pthis->data[_byte_count] = can1_tp_value.TP_RX_MSG.tp_rx_msg.data[_byte_count];
		}
		pthis->SA = can1_tp_value.TP_RX_MSG.tp_rx_msg.SA;
		pthis->byte_count = can1_tp_value.TP_RX_MSG.tp_rx_msg.byte_count;
		pthis->PGN = can1_tp_value.TP_RX_MSG.tp_rx_msg.PGN;

		/*丢弃转存过的数据*/
		can1_tp_value.TP_RX_MSG.tp_rx_msg.byte_count = 0;
		can1_tp_value.TP_RX_MSG.tp_rx_msg.PGN = 0;

		return RC_SUCCESS;
		break;

	case Select_CAN_NODE_2:
		/*能否获取数据*/
		if (can2_tp_value.TP_Flag.state != J1939_TP_NULL || can2_tp_value.TP_RX_MSG.tp_rx_msg.PGN == 0)
		{
			return RC_ERROR;
		} /*缓存区大小是否够*/
		if ((pthis->data_num) < can2_tp_value.TP_RX_MSG.tp_rx_msg.byte_count)
		{
			return RC_ERROR;
		}

		/*转存数据*/
		for (_byte_count = 0; _byte_count < pthis->data_num; _byte_count++)
		{
			pthis->data[_byte_count] = can2_tp_value.TP_RX_MSG.tp_rx_msg.data[_byte_count];
		}
		pthis->SA = can2_tp_value.TP_RX_MSG.tp_rx_msg.SA;
		pthis->byte_count = can2_tp_value.TP_RX_MSG.tp_rx_msg.byte_count;
		pthis->PGN = can2_tp_value.TP_RX_MSG.tp_rx_msg.PGN;

		/*丢弃转存过的数据*/
		can2_tp_value.TP_RX_MSG.tp_rx_msg.byte_count = 0;
		can2_tp_value.TP_RX_MSG.tp_rx_msg.PGN = 0;

		return RC_SUCCESS;
		break;
	default:
		return RC_PARAMERROR;
		break;
	}
}
